clc;clear;close all;

t0=0;tf=3;dt=0.02;
a=0.025;
m1=0.5;m2=0.5;l1=0.1;l2=0.1
i=1;

for t=t0:dt:tf
    time(i)=t;
    if(t>=0&&t<=1)
        q(i)=0.1+0.5*a*t*t; %末端位置
        dq(i)=a*t;        
        %tao1(i)=(m1*l1*l1+m2*(l1*l1+2*l1*l2*cos(theta2)+l2*l2))*ddtheta1+m2*(l1*l2*cos(theta2)+l2*l2)*ddtheta2-m2*l1*l2*sin(theta2)*(2*dtheta1+dtheta2)*dtheta2+(m1+m2)*l1*9.8*cos(theta1)+m2*l2*9.8*cos(theta1+theta2);
        %tao2(i)=m2*(l1*l2*cos(theta2)+l2*l2)*ddtheta1+m2*l2*l2*ddtheta2+m2*l2*l1*sin(theta2)*dtheta1*dtheta1+m2*9.8*l2*cos(theta1+theta2);
    elseif(t>=1&&t<=2)
        q(i)=0.1125+0.025*(t-1);
        dq(i)=0.025;        
    else
        q(i)=0.1375+0.025*(t-2)-0.5*a*(t-2)*(t-2);
        dq(i)=0.075-a*t;
    end
    theta1(i)=acos(q(i)*5);
    theta2(i)=-2*theta1(i);
    dtheta1(i)=-5/sqrt(1-25*q(i)*q(i));
    dtheta2(i)=-2*dtheta1(i);
    ddtheta1(i)=-250*q(i)/(1-25*q(i)*q(i));
    ddtheta2(i)=-2*ddtheta1(i);
    tao1(i)=(m1*l1*l1+m2*(l1*l1+2*l1*l2*cos(theta2(i))+l2*l2))*ddtheta1(i)+m2*(l1*l2*cos(theta2(i))+l2*l2)*ddtheta2(i)-m2*l1*l2*sin(theta2(i))*(2*dtheta1(i)+dtheta2(i))*dtheta2(i)+(m1+m2)*l1*9.8*cos(theta1(i))+m2*l2*9.8*cos(theta1(i)+theta2(i));
    tao2(i)=m2*(l1*l2*cos(theta2(i))+l2*l2)*ddtheta1(i)+m2*l2*l2*ddtheta2(i)+m2*l2*l1*sin(theta2(i))*dtheta1(i)*dtheta1(i)+m2*9.8*l2*cos(theta1(i)+theta2(i));   
    i=i+1;
end

figure(1)
subplot(1,2,1)
plot(time,q)
title('位置曲线')
grid on
subplot(1,2,2)
plot(time,dq)
title('速度曲线')
grid on
figure(2)
subplot(1,2,1)
plot(time,dtheta1*180/pi)
title('dtheta1')
grid on
subplot(1,2,2)
plot(time,dtheta2*180/pi)
title('dtheta2')
grid on
figure(3)
subplot(1,2,1)
plot(time,tao1)
title('关节1力矩曲线')
grid on
subplot(1,2,2)
plot(time,tao2)
title('关节2力矩曲线')
grid on